#include <stdio.h>
#include "motor.h"
#include "port_config.h"
#include "port_encoder.h"
#include "port_pwm.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/queue.h"
#include "freertos/semphr.h"
#include "port_gpio.h"

typedef struct {
    xSemaphoreHandle semaphore;
    int speed_limit_max;
    int speed_limit_min;
    int speed;
    uint8_t gpio_forward_port;
    uint8_t gpio_reverse_port;
    uint8_t encoder_port;
}motor_t;

#define MOTOR_MAX_NUM (2)
#define IS_MOTOR_CH(ch) ((ch) < MOTOR_MAX_NUM)
static motor_t g_motor[MOTOR_MAX_NUM] = {
    {
        .speed_limit_max = 100,
        .speed_limit_min = -100,
        .gpio_forward_port = MOTOR_LEFT_FORWARD_PORT,
        .gpio_reverse_port = MOTOR_LEFT_REVERSE_PORT,
        .encoder_port = MOTOR_LEFT_ENC_PORT,
    },
    {
        .speed_limit_max = 100,
        .speed_limit_min = -100,
        .gpio_forward_port = MOTOR_RIGHT_FORWARD_PORT,
        .gpio_reverse_port = MOTOR_RIGHT_REVERSE_PORT,
        .encoder_port = MOTOR_RIGHT_ENC_PORT,
    }
};

static int motor_speed_limit_filter(motor_t *motor, int speed)
{
    if(speed > motor->speed_limit_max)
    {
        speed = motor->speed_limit_max;
    }
    else if(speed < motor->speed_limit_min)
    {
        speed = motor->speed_limit_min;
    }
    return speed;
}

void motor_init(void)
{
    encoder_init();
    pwm_out_init();

    g_motor[0].semaphore = xSemaphoreCreateBinary();
    g_motor[1].semaphore = xSemaphoreCreateBinary();
}

void motor_speed_update(void)
{
    for(uint8_t ch=0; ch<MOTOR_MAX_NUM; ch++)
    {
        int speed = encoder_get_counter(g_motor[ch].encoder_port);
        encoder_clear(g_motor[ch].encoder_port);
        speed = motor_speed_limit_filter(&g_motor[ch], speed);
        xSemaphoreTake(g_motor[ch].semaphore, portMAX_DELAY);
        g_motor[ch].speed = speed;
        xSemaphoreGive(g_motor[ch].semaphore);
    }
}

void motor_set_pwm(uint8_t ch, int pwm)
{
    if(IS_MOTOR_CH(ch))
    {
        if(pwm>0)
        {
            gpio_set(g_motor[ch].gpio_forward_port, 1);
            gpio_set(g_motor[ch].gpio_reverse_port, 0);
        }
        else
        {
            gpio_set(g_motor[ch].gpio_forward_port, 0);
            gpio_set(g_motor[ch].gpio_reverse_port, 1);
        }
        pwm_out_set(ch, pwm);
    }
}

int motor_speed_get(uint8_t ch)
{
    int speed = 0;
    if(IS_MOTOR_CH(ch))
    {
        xSemaphoreTake(g_motor[ch].semaphore, portMAX_DELAY);
        speed = g_motor[ch].speed;
        xSemaphoreGive(g_motor[ch].semaphore);
    }
    return speed;
}